#include "defaults.h"
#include "nurbs.h"
#include "visualdebugger.h"
#include <iostream>

VisualDebugger::VisualDebugger() {
	
}

VisualDebugger::~VisualDebugger() {

}

void VisualDebugger::RenderGrid() const {
	const size_t n = 21;

	glPushAttrib(GL_ENABLE_BIT | GL_CURRENT_BIT | GL_LINE_BIT);
	glEnable(GL_BLEND);
	glEnable(GL_LINE_SMOOTH);
	
	glColor3f(135.0f / 255.0f, 135.0f / 255.0f, 135.0f / 255.0f);
	glLineWidth(1.0);
	glDisable(GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);
	glBegin(GL_LINES);
	for(size_t i = 0; i < n; i++) {
		nfloat f = (nfloat)i - 0.5 * (n - 1);
		if(i != (n - 1) / 2) {
			glVertex3d(f, 0.0, -0.5 * (n - 1));
			glVertex3d(f, 0.0, 0.5 * (n - 1));
			glVertex3d(-0.5 * (n - 1), 0.0, f);
			glVertex3d(0.5 * (n - 1), 0.0, f);
		}
	}
	glEnd();

	glColor3f(0.0f, 0.0f, 0.0f);
	glBegin(GL_LINES);
	glVertex3d(0.0, 0.0, -0.5 * (n - 1));
	glVertex3d(0.0, 0.0, 0.5 * (n - 1));
	glVertex3d(-0.5 * (n - 1), 0.0, 0.0);
	glVertex3d(0.5 * (n - 1), 0.0, 0.0);
	glEnd();

	glDisable(GL_LINE_SMOOTH);
	glPopAttrib();
}

void VisualDebugger::RenderAxis(int width, int height, const Vec3n& eye, const Vec3n& lookat, const Vec3n& up) const {
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0, (GLdouble)width/100.0, 0, (GLdouble)height/100.0,0.0,128.0);
	Vec2n pos =  math::V2n(0.4, 0.4);

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();

	glPushAttrib(GL_ENABLE_BIT | GL_CURRENT_BIT | GL_LINE_BIT);
	glDisable(GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);
	glDisable(GL_DEPTH_TEST);
	glEnable(GL_BLEND);
	glEnable(GL_LINE_SMOOTH);
	
	glTranslatef(pos[0], pos[1], -3.0);
	Mat4x4n R;
	R.Identity();
	R.Lookat(eye[0], eye[1], eye[2], lookat[0], lookat[1], lookat[2], up[0], up[1], up[2]);
	R[12] = R[13] = R[14] = 0.0;
	glMultMatrixd(R.GetData());

	nfloat s = 0.2;

	glColor3f(1.0,0.0,0.0);
	glBegin(GL_LINES);
		glVertex3f(0.0,0.0,0.0);
		glVertex3f(s,0.0,0.0);
	glEnd();

	glColor3f(0.0,0.6,0.329);
	glBegin(GL_LINES);
		glVertex3f(0.0,0.0,0.0);
		glVertex3f(0.0,s,0.0);
	glEnd();

	glColor3f(0.0,0.0,1.0);
	glBegin(GL_LINES);
		glVertex3f(0.0,0.0,0.0);
		glVertex3f(0.0,0.0,s);
	glEnd();

	glPushMatrix();
	glLoadIdentity();
	glTranslatef(pos[0], pos[1], -3.0);

	Vec4n Xaxis = R * math::V4n(s + 0.025, 0.0, 0.0, 1.0);
	Vec4n Yaxis = R * math::V4n(0.0, s + 0.025, 0.0, 1.0);
	Vec4n Zaxis = R * math::V4n(0.0, 0.0, s + 0.025, 1.0);

	glLineWidth(2.0);
	glPushMatrix();
	glTranslatef(Xaxis[0], Xaxis[1], Xaxis[2]);
	glColor3f(1.0,0.0,0.0);
	glBegin(GL_LINES);
		glVertex3f(-0.03,0.05,0.0);
		glVertex3f(0.03,-0.05,0.0);
		glVertex3f(0.03,0.05,0.0);
		glVertex3f(-0.03,-0.05,0.0);
	glEnd();
	glPopMatrix();

	glPushMatrix();
	glTranslatef(Yaxis[0], Yaxis[1], Yaxis[2]);
	glColor3f(0.0,0.6,0.329);
	glBegin(GL_LINES);
		glVertex3f(-0.03,0.04,0.0);
		glVertex3f(0.03,0.04,0.0);
		glVertex3f(0.03,0.04,0.0);
		glVertex3f(-0.03,-0.04,0.0);
		glVertex3f(-0.03,-0.04,0.0);
		glVertex3f(0.03,-0.04,0.0);
	glEnd();
	glPopMatrix();

	glPushMatrix();
	glTranslatef(Zaxis[0], Zaxis[1], Zaxis[2]);
	glColor3f(0.0,0.0,1.0);
	glBegin(GL_LINES);
		glVertex3f(-0.03,0.05,0.0);
		glVertex3f(0.0,0.01,0.0);
		glVertex3f(0.03,0.05,0.0);
		glVertex3f(0.0,0.01,0.0);
		glVertex3f(0.0,0.01,0.0);
		glVertex3f(0.0,-0.04,0.0);
	glEnd();
	glPopMatrix();

	glPopMatrix();
	glLineWidth(1.0);
		
	glDisable(GL_LINE_SMOOTH);
	
	glPopAttrib();

	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
}

void VisualDebugger::AddPoint(const Vec3n& position, const Vec3n& color) {
	boost::tuples::tuple<Vec3n, Vec3n> t(position, color);
	point.push_back(t);
}

void VisualDebugger::AddSphere(const Vec3n& position, const Vec3n& color, nfloat diameter) {
	boost::tuples::tuple<Vec3n, Vec3n, nfloat> t(position, color, diameter);
	sphere.push_back(t);
}

void VisualDebugger::AddCross(const Vec3n& position, const Vec3n& color, nfloat diameter) {
	boost::tuples::tuple<Vec3n, Vec3n, nfloat> t(position, color, diameter);
	cross.push_back(t);
}

void VisualDebugger::AddLineSegment(const Vec3n& positionA, const Vec3n& positionB, const Vec3n& color) {
	boost::tuples::tuple<Vec3n, Vec3n, Vec3n> t(positionA, positionB, color);
	linesegment.push_back(t);
}

void VisualDebugger::AddTriangle(const Vec3n& positionA, const Vec3n& positionB, const Vec3n& positionC, const Vec3n& color) {
	boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n> t(positionA, positionB, positionC, color);
	triangle.push_back(t);
}
	
void VisualDebugger::AddTriangle(const std::pair< std::vector<Vec3n>, std::vector<size_t> >& tm, const Vec3n& color) {
	for(size_t i = 0; i < tm.second.size(); i += 3) {
		size_t a = tm.second[i];
		size_t b = tm.second[i + 1];
		size_t c = tm.second[i + 2];
		boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n> t(tm.first[a], tm.first[b], tm.first[c], color);
		triangle.push_back(t);
	}
}

void VisualDebugger::AddTetraHedron(const Vec3n& positionA, const Vec3n& positionB, const Vec3n& positionC, const Vec3n& positionD, const Vec3n& color) {
	boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n, Vec3n> t(positionA, positionB, positionC, positionD, color);
	tetrahedron.push_back(t);
}
	
void VisualDebugger::AddQuadrilateral(const Vec3n& positionA, const Vec3n& positionB, const Vec3n& positionC, const Vec3n& positionD, const Vec3n& color) {
	boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n, Vec3n> q(positionA, positionB, positionC, positionD, color);
	quadrilateral.push_back(q);
}

void VisualDebugger::AddPath(const std::vector<Vec3n>& path, const Vec3n& color) {
	boost::tuples::tuple< std::vector<Vec3n>, Vec3n> p(path, color);
	this->path.push_back(p);
}

void VisualDebugger::Clear() {
	ClearPoints();
	ClearSpheres();
	ClearLineSegments();
	ClearTriangles();
	ClearTetraHedra();
	ClearQuadrilaterals();
	ClearPaths();
    ClearCrosss();
}

void VisualDebugger::ClearPoints() {
	point.clear();
}

void VisualDebugger::ClearSpheres() {
	sphere.clear();
}

void VisualDebugger::ClearLineSegments() {
	linesegment.clear();
}

void VisualDebugger::ClearTriangles() {
	triangle.clear();
}

void VisualDebugger::ClearTetraHedra() {
	tetrahedron.clear();
}

void VisualDebugger::ClearQuadrilaterals() {
	quadrilateral.clear();
}

void VisualDebugger::ClearPaths() {
	path.clear();
}

void VisualDebugger::ClearCrosss() {
	cross.clear();
}

void VisualDebugger::Render() const {
	glPushAttrib(GL_ENABLE_BIT);
	glDisable(GL_TEXTURE_2D);

	glPushAttrib(GL_POINT_BIT);
	glPointSize(2.5);
	glBegin(GL_POINTS);
	for(size_t i = 0; i < point.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n>& t = point[i];
		const Vec3n& P = boost::tuples::get<0>(t);
		const Vec3n& C = boost::tuples::get<1>(t);
		glColor3d(C[0], C[1], C[2]);
		glVertex3f(P[0], P[2], P[1]);
	}
	glEnd();
	glPopAttrib();

	glPushAttrib(GL_LINE_BIT | GL_ENABLE_BIT);
	glDisable(GL_BLEND);
	glLineWidth(1.0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < linesegment.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, Vec3n>& t = linesegment[i];
		const Vec3n& P1 = boost::tuples::get<0>(t);
		const Vec3n& P2 = boost::tuples::get<1>(t);
		const Vec3n& C = boost::tuples::get<2>(t);
		glColor3d(C[0], C[1], C[2]);
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
	}
	glEnd();
	glPopAttrib();

	glPushAttrib(GL_LINE_BIT | GL_ENABLE_BIT);
	glDisable(GL_BLEND);
	glLineWidth(1.0);
	for(size_t i = 0; i < path.size(); i++) {
		const boost::tuples::tuple< std::vector<Vec3n>, Vec3n>& t = path[i];
		const std::vector<Vec3n>& P = boost::tuples::get<0>(t);
		const Vec3n& C = boost::tuples::get<1>(t);

		glColor3d(C[0], C[1], C[2]);
		glBegin(GL_LINE_STRIP);
		for(size_t j = 0; j < P.size(); j++)
			glVertex3f(P[j][0], P[j][2], P[j][1]);
		glEnd();
	}
	glPopAttrib();
	
	glPushAttrib(GL_POLYGON_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glBegin(GL_TRIANGLES);
	for(size_t i = 0; i < triangle.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n>& t = triangle[i];
		const Vec3n& P1 = boost::tuples::get<0>(t);
		const Vec3n& P2 = boost::tuples::get<1>(t);
		const Vec3n& P3 = boost::tuples::get<2>(t);
		const Vec3n& C = boost::tuples::get<3>(t);
		glColor3d(C[0], C[1], C[2]);
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P3[0], P3[2], P3[1]);
	}
	glEnd();
	glPopAttrib();
	
	glPushAttrib(GL_POLYGON_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glBegin(GL_LINES);
	for(size_t i = 0; i < cross.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, nfloat>& t = cross[i];
		const Vec3n& P = boost::tuples::get<0>(t);
		const Vec3n& C = boost::tuples::get<1>(t);
		const nfloat& diameter = boost::tuples::get<2>(t);
		glColor3d(C[0], C[1], C[2]);
		glVertex3f(P[0] - diameter, P[2], P[1]);
		glVertex3f(P[0] + diameter, P[2], P[1]);
		glVertex3f(P[0], P[2] - diameter, P[1]);
		glVertex3f(P[0], P[2] + diameter, P[1]);
		glVertex3f(P[0], P[2], P[1] - diameter);
		glVertex3f(P[0], P[2], P[1] + diameter);
	}
	glEnd();
	glPopAttrib();
	
	glPushAttrib(GL_POLYGON_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glEnable(GL_LIGHTING);
	for(size_t i = 0; i < sphere.size(); i++) {	
		const boost::tuples::tuple<Vec3n, Vec3n, nfloat>& t = sphere[i];
		const Vec3n& P = boost::tuples::get<0>(t);
		const Vec3n& C = boost::tuples::get<1>(t);
		const nfloat& diameter = boost::tuples::get<2>(t);
//		glColor3d(C[0], C[1], C[2]);
		GLfloat mat[4] = {C[0], C[1], C[2], 1.0};
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, mat); 
		glPushMatrix();
		glTranslatef(P[0], P[2], P[1]);
		glutSolidSphere(diameter, 10, 10);
		glPopMatrix();
	}
	glPopAttrib();

	glBegin(GL_TRIANGLES);
	for(size_t i = 0; i < tetrahedron.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n, Vec3n>& t = tetrahedron[i];
		const Vec3n& P1 = boost::tuples::get<0>(t);
		const Vec3n& P2 = boost::tuples::get<1>(t);
		const Vec3n& P3 = boost::tuples::get<2>(t);
		const Vec3n& P4 = boost::tuples::get<3>(t);
		const Vec3n& C = boost::tuples::get<4>(t);
		glColor3d(C[0], C[1], C[2]);

		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P3[0], P3[2], P3[1]);
		
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P4[0], P4[2], P4[1]);
		
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P3[0], P3[2], P3[1]);
		glVertex3f(P4[0], P4[2], P4[1]);
		
		glVertex3f(P3[0], P3[2], P3[1]);
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P4[0], P4[2], P4[1]);
	}
	glEnd();

	glPushAttrib(GL_POLYGON_BIT);
	glPushAttrib(GL_ENABLE_BIT);
	glPushAttrib(GL_LIGHTING_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glPolygonOffset(1.0, 1.0);
	glEnable(GL_POLYGON_OFFSET_FILL);
	glEnable(GL_LIGHTING);
	glEnable(GL_COLOR_MATERIAL);
	glBegin(GL_QUADS);
	for(size_t i = 0; i < quadrilateral.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n, Vec3n>& q = quadrilateral[i];
		const Vec3n& P1 = boost::tuples::get<0>(q);
		const Vec3n& P2 = boost::tuples::get<1>(q);
		const Vec3n& P3 = boost::tuples::get<2>(q);
		const Vec3n& P4 = boost::tuples::get<3>(q);
		const Vec3n& c = boost::tuples::get<4>(q);
		glColor3f(c[0], c[1], c[2]);

		Vec3n u = P2 - P1;
		Vec3n v = P3 - P1;
		Vec3n w = P4 - P1;

		normalize(u);
		normalize(v);
		normalize(w);

		Vec3n n1 = math::cross(u, v);
		Vec3n n2 = math::cross(v, w);
		Vec3n n3 = -0.5 * (n1 + n2);
		normalize(n3);

		glNormal3f(n3[0], n3[2], n3[1]);
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P3[0], P3[2], P3[1]);
		glVertex3f(P4[0], P4[2], P4[1]);
	}
	glEnd();
	glDisable(GL_LIGHTING);
	glDisable(GL_POLYGON_OFFSET_FILL);
	glPopAttrib();
	glPopAttrib();
	glPopAttrib();


	glPushAttrib(GL_POLYGON_BIT);
	glPushAttrib(GL_LINE_BIT);
	glPushAttrib(GL_ENABLE_BIT);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glLineWidth(1.0);
	glBegin(GL_QUADS);
	for(size_t i = 0; i < quadrilateral.size(); i++) {
		const boost::tuples::tuple<Vec3n, Vec3n, Vec3n, Vec3n, Vec3n>& q = quadrilateral[i];
		const Vec3n& P1 = boost::tuples::get<0>(q);
		const Vec3n& P2 = boost::tuples::get<1>(q);
		const Vec3n& P3 = boost::tuples::get<2>(q);
		const Vec3n& P4 = boost::tuples::get<3>(q);
		const Vec3n& C = boost::tuples::get<4>(q);
		glColor3d(0.0, 0.0, 0.0);
		glVertex3f(P1[0], P1[2], P1[1]);
		glVertex3f(P2[0], P2[2], P2[1]);
		glVertex3f(P3[0], P3[2], P3[1]);
		glVertex3f(P4[0], P4[2], P4[1]);
	}
	glEnd();
	glPopAttrib();
	glPopAttrib();
	glPopAttrib();

	glPopAttrib();
}
	
void VisualDebugger::RenderCircle(nfloat radius, const Vec3n& color, size_t n) const {
	glBegin(GL_LINE_LOOP);
	glColor3f(color[0], color[1], color[2]);
	for(size_t i = 0; i < n; i++) {
		nfloat angle = (nfloat)(2.0 * M_PI * i) / n;
		glVertex3f(radius * cos(angle), 0.0, radius *sin(angle));
	}
	glEnd();
}

void VisualDebugger::RenderCircle(nfloat radius, size_t n) const {
	glBegin(GL_LINE_LOOP);
	for(size_t i = 0; i < n; i++) {
		nfloat angle = (nfloat)(2.0 * M_PI * i) / n;
		glVertex3f(radius * cos(angle), 0.0, radius *sin(angle));
	}
	glEnd();
}

void VisualDebugger::RenderOrbit(const Vec3n& eye, const Vec3n& lookat) const {
	glPushAttrib(GL_ENABLE_BIT | GL_CURRENT_BIT | GL_LINE_BIT);
	Vec4n color1 = math::V4n(1.0, 1.0, 1.0, 0.75);
	Vec4n color2 = math::V4n(1.0, 0.6, 0.0, 0.8);
	Vec4n color3 = math::V4n(1.0, 0.6, 0.0, 0.8);
	nfloat radius = 0.1 * length(eye - lookat);

	glLineWidth(4.0);
	glDisable(GL_TEXTURE_2D);
	glDisable(GL_LIGHTING);

	glBegin(GL_LINES);
	glColor4f(color1[0], color1[1], color1[2], color1[3]);
	glVertex3f(lookat[0] - radius, lookat[1], lookat[2]);
	glVertex3f(lookat[0] + radius, lookat[1], lookat[2]);
	glVertex3f(lookat[0], lookat[1] - radius, lookat[2]);
	glVertex3f(lookat[0], lookat[1] + radius, lookat[2]);
	glVertex3f(lookat[0], lookat[1], lookat[2] - radius);
	glVertex3f(lookat[0], lookat[1], lookat[2] + radius);
	glEnd();

	glPushMatrix();
	glTranslatef(lookat[0], lookat[1], lookat[2]);
	glColor4f(color2[0], color2[1], color2[2], color2[3]);
	visualDebugger.RenderCircle(radius, 40);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(lookat[0], lookat[1], lookat[2]);
	glRotatef(90.0f, 1.0f, 0.0f, 0.0f);
	glColor4f(color3[0], color3[1], color3[2], color3[3]);
	visualDebugger.RenderCircle(radius, 40);
	glPopMatrix();
	glPopAttrib();
}

